# -*- coding: utf-8 -*-
'''
Created on 15.12.2019

@author: yu03
'''
import numpy as np
import os
import re
import matplotlib.pyplot as plt
from scipy.optimize import curve_fit
import glob
from mpl_toolkits.mplot3d import Axes3D
from PyUeye_Unified.Cross_200line_SAAC import folder_path, np_result_names, hor_index, ver_index, hor_lines, ver_lines
import sys
from FFT_Interpolation import FFT_interpolation_nonlinearity_compare
from matplotlib import cm
from matplotlib.ticker import LinearLocator, FormatStrFormatter

# # plt_show_mode = 'show' ### plt.show()
# plt_show_mode = 'save' ### plt.save()
# name_raw = r'G:\3DoF_Interferometer_SAAC\Range_test_doubleway\28x28Grids\D_28x28_Spez_2DInt_Liang_ImMB_w1_F_EN.raw'
# name_raw_r = r'G:\3DoF_Interferometer_SAAC\Range_test_doubleway\28x28Grids\D_28x28_Spez_2DInt_Liang_ImMB_w1_R_EN.raw'

#name_encoder_f = folder_path + '\\' +  'F_en' + '.raw'
#name_encoder_r = folder_path + '\\' +  'R_en' + '.raw'
SAAC_h_set, SAAC_v_set = [], []
SAAC_raw_data = glob.glob(folder_path+'\*.raw')
print(len(SAAC_raw_data))
for i in SAAC_raw_data:
#    print(i.strip().split('\\')[-1])
#    print(i.strip().split('\\')[-1].strip().split('_'))
    data_name = i.strip().split('\\')[-1].strip().split('_')
#    print(data_name[-1])
    if data_name[-1] == 'AH.raw':
        SAAC_h_set.append(i)
    elif data_name[-1] == 'AV.raw':
        SAAC_v_set.append(i)


def read_encoder(name_raw):
    print('Reading Data')
    with open(name_raw,'r') as fid:
        line='%'
        while line[0:3] != '%iP':
            line = fid.readline()
#             print(line)
        out_str = fid.readlines()
    iP, iH, iV, iA, cW,  EN_c,  EN_t,  EN_iP_H,  EN_iP_V,  EN_iP_A,  EN_H,  EN_V,  EN_A = [], [], [], [], [], [], [], [], [], [], [], [], []
    for line in out_str:
        a, b, c, d, e, f, g, h, i, j, k, l, m= line
        EN_t.append(float(g)) # timestamp
        EN_H.append(float(k))
        EN_V.append(float(l))   
    EN_t = np.array(EN_t)
    EN_H = np.array(EN_H)
    EN_V = np.array(EN_V)
    return EN_t, EN_H, EN_V

def read_SAAC(name_raw):
    print('Reading Data')
    with open(name_raw,'r') as fid:
        line='%'
        while line[0:3] != '%iP':
            line = fid.readline()
#             print(line)
        out_str = fid.readlines()
    iP, iH, iV, iA, cW, AC_cCli, AC_tCli, AC_3, AC_003, AC_X, AC_Y = [], [], [], [], [], [], [], [], [], [], []
    for line in out_str:
        a, b, c, d, e, f, g, h, i, j, k= line.strip().split(' ')
        AC_tCli.append(float(g)) # timestamp
        AC_X.append(float(j))
        AC_Y.append(float(k))   
    AC_tCli = np.array(AC_tCli)
    AC_X = np.array(AC_X)
    AC_Y = np.array(AC_Y)
    return AC_tCli, AC_X

if 0:
    encoder_f_time, encoder_f_h, encoder_f_v = read_encoder(name_encoder_f)
    encoder_r_time, encoder_r_h, encoder_r_v = read_encoder(name_encoder_r)
    encoder_time = np.concatenate((encoder_f_time, encoder_r_time))
    encoder_h = np.concatenate((encoder_f_h, encoder_r_h))
    encoder_v = np.concatenate((encoder_f_v, encoder_r_v))
    encoder_time = encoder_time/1e3 #in second

    SAAC_f_h_time, SAAC_f_h = read_SAAC(name_SAAC_f_h)
    SAAC_r_h_time, SAAC_r_h = read_SAAC(name_SAAC_r_h)
    SAAC_h_time = np.concatenate((SAAC_f_h_time, SAAC_r_h_time))
    SAAC_h = np.concatenate((SAAC_f_h, SAAC_r_h))
    SAAC_h_time = SAAC_h_time/1e3 # in second
    
    SAAC_f_v_time, SAAC_f_v = read_SAAC(name_SAAC_f_v)
    SAAC_r_v_time, SAAC_r_v = read_SAAC(name_SAAC_r_v)
    SAAC_v_time = np.concatenate((SAAC_f_v_time, SAAC_r_v_time))
    SAAC_v = np.concatenate((SAAC_f_v, SAAC_r_v))
    SAAC_v_time = SAAC_v_time/1e3 # in second

SAAC_h, SAAC_v, SAAC_h_time, SAAC_v_time = np.empty(1), np.empty(1), np.empty(1), np.empty(1)
for i in range(len(SAAC_h_set)):
    SAAC_h_time = np.concatenate((SAAC_h_time, read_SAAC(SAAC_h_set[i])[0]))
    SAAC_h = np.concatenate((SAAC_h, read_SAAC(SAAC_h_set[i])[1]))
    
for i in range(len(SAAC_v_set)):
    SAAC_v_time = np.concatenate((SAAC_v_time, read_SAAC(SAAC_v_set[i])[0]))
    SAAC_v = np.concatenate((SAAC_v, read_SAAC(SAAC_v_set[i])[1]))

SAAC_h_time = SAAC_h_time[1:]
SAAC_h = SAAC_h[1:]
SAAC_v_time = SAAC_v_time[1:]
SAAC_v = SAAC_v[1:]

SAAC_h_time = SAAC_h_time/1e3 # in second
SAAC_v_time = SAAC_v_time/1e3 # in second

i = 0
fs_cam = 25
hor_angle_avr_set = []
ver_angle_avr_set = []
hor_length_avr_set = []
ver_length_avr_set = []
time_set = []
''' 
    for all groups
''' 
for np_result in np_result_names:
    ''' 
        reading .npy file
    ''' 
    file_name = np_result.split('\\')[-1] ### x_lines.py
#     file_name = re.findall(r"openmode\\(.+).npy", np_result)[0]
    print(file_name)
    i += 1
    f = open(np_result, 'rb')
    f_size = os.fstat(f.fileno()).st_size
    ''' 
        put 4-D results in "lines"
    ''' 
    lines = []
    time_sequence = []
    while f.tell() < f_size:
        line = np.load(f, allow_pickle=True)
        lines.append(line)
    print('%i: %s, %.1fMB'%(i, file_name, f_size/1e6))
    time_sequence = lines[4]
    lines = np.array(lines[0:4])
    print(np.shape(lines), 'Channle, lines, frames:')
    frame_num = np.shape(lines)[2]
    line_num = np.shape(lines)[1]
    
#     print(np.shape(lines))
    hor_angle_set, ver_angle_set, hor_length_set, ver_length_set = lines
    time_set.append(time_sequence)
    
    ''' 
        making average
    ''' 
    averaged_results = []
    
#     index_line = 0
#     hor_angle_avr = hor_angle_set[index_line]
#     ver_angle_avr = ver_angle_set[index_line]
#     hor_length_avr = hor_length_set[index_line]
#     ver_length_avr = ver_length_set[index_line]
    
    hor_angle_avr = np.mean(hor_angle_set, axis=0)
    ver_angle_avr = np.mean(ver_angle_set, axis=0)
    hor_length_avr = np.mean(hor_length_set, axis=0)
    ver_length_avr = np.mean(ver_length_set, axis=0)
    
    hor_angle_avr_set.append(hor_angle_avr)
    ver_angle_avr_set.append(ver_angle_avr)
    hor_length_avr_set.append(hor_length_avr)
    ver_length_avr_set.append(ver_length_avr)

time_set = np.array(time_set)
time_set = np.hstack(time_set)  
time_set = time_set + 0.3

hor_angle_avr_set = np.array(hor_angle_avr_set)
ver_angle_avr_set = np.array(ver_angle_avr_set)
hor_length_avr_set = np.array(hor_length_avr_set)
ver_length_avr_set = np.array(ver_length_avr_set)

hor_angle_avr_set = np.hstack(hor_angle_avr_set)  
ver_angle_avr_set = np.hstack(ver_angle_avr_set)
hor_length_avr_set = np.hstack(hor_length_avr_set)
ver_length_avr_set = np.hstack(ver_length_avr_set)

hor_angle_avr_set = hor_angle_avr_set * 0.20626479821012 # in arcsecond
ver_angle_avr_set = ver_angle_avr_set * 0.20626479821012 # in arcsecond

hor_angle_avr_set = (hor_angle_avr_set - hor_angle_avr_set[0]) * 0.969 + hor_angle_avr_set[0]
ver_angle_avr_set = (ver_angle_avr_set - ver_angle_avr_set[0]) * 0.97 + ver_angle_avr_set[0]

#encoder_h = encoder_h * -1 - 200
#encoder_v = encoder_v * -1
hor_angle_avr_set = hor_angle_avr_set + 257 + 2 + 5
ver_angle_avr_set = ver_angle_avr_set * -1 - 218 +21
# for k in range(5):
#     print(time_set[k])
#print(len(encoder_h))
print(len(SAAC_h), SAAC_h[1], SAAC_h[-1])
print(len(SAAC_h_time), SAAC_h_time[1], SAAC_h_time[-1])

print(len(SAAC_v), SAAC_v[0], SAAC_v[-1])
print(len(SAAC_v_time), SAAC_v_time[0], SAAC_v_time[-1])

print(len(hor_angle_avr_set), hor_angle_avr_set[0], hor_angle_avr_set[-1])
print(len(time_set), time_set[0], time_set[-1])


#print(time_set[-1]-time_set[0])
#print(SAAC_h_time[-1]-SAAC_h_time[0])
fig = plt.figure('Raw Data')
plt.gcf().set_size_inches(18,9)

ax1 = fig.add_subplot(2, 2, 1)
ax2 = fig.add_subplot(2, 2, 2)
ax3 = fig.add_subplot(2, 2, 3)
ax4 = fig.add_subplot(2, 2, 4)

ax1.plot(time_set, hor_angle_avr_set, color='black', marker=' ', markersize=1, label='horizontal angle')
ax2.plot(time_set, hor_length_avr_set, color='black', marker=' ', markersize=1, label='horizontal length')
ax3.plot(time_set, ver_angle_avr_set, color='black', label='vertical angle')
ax4.plot(time_set, ver_length_avr_set, color='black', label='vertical length')

#ax1.plot(encoder_time, encoder_h, color='blue', marker=' ', markersize=1, label='encoder')
ax1.plot(SAAC_h_time, SAAC_h, color='red', marker=' ', markersize=1, label='SAAC')
#ax3.plot(encoder_time, encoder_v, color='blue', marker=' ', markersize=1, label='encoder')
ax3.plot(SAAC_v_time, SAAC_v, color='red', marker='', markersize=1, label='SAAC')

ax1.title.set_text('Horizontal Tilt')
ax2.title.set_text('Horizontal Length')
ax3.title.set_text('Vertical Tilt')
ax4.title.set_text('Vertical Length')
   
ax1.set_ylabel('Horizontal Tilt \ acrsec')
ax1.set_xlabel('Timestamp')
   
ax2.set_ylabel('Horizontal Length \ nm')
ax2.set_xlabel('Frame Num.')
   
ax3.set_ylabel('Vertical Tilt \ arcsec')
ax3.set_xlabel('Timestamp')
   
ax4.set_ylabel('Vertical Length \ nm')
ax4.set_xlabel('Frame Num.')

ax1.grid(which='both', axis='both')
ax2.grid(which='both', axis='both')
ax3.grid(which='both', axis='both')
ax4.grid(which='both', axis='both')

ax1.legend()
ax2.legend()
ax3.legend()
ax4.legend()

plt.tight_layout()


fig = plt.figure('Timestamp')
plt.subplot(2,1,1)

# t1 = time_set
# t2 = encoder_time
# t0 = encoder_time[0]
# print(t1[0,t2[-1])
# tmp = t1>=t0
# t1 = t1[tmp]
# t2 = t2 - t2[0]
# t1 = time_set[:len(t2)]
# t1 = t1 - t1[0]
# idx = np.arange(len(t2))
# plt.plot(idx,t1,idx,t2, color='black', label='sensor')
plt.plot(time_set, color='red', label='sensor')
#plt.plot(encoder_time, color='blue', label='encoder')
plt.plot(SAAC_h_time, color='black', label='SAAC_h')
plt.plot(SAAC_v_time, color='green', label='SAAC_v')
plt.grid()
plt.legend()

# encoder_time = encoder_time[3000:]
plt.subplot(2,1,2)
plt.plot(time_set-(time_set[0]+(time_set[-1]-time_set[0])/len(time_set)*np.arange(len(time_set))), color='red', label='sensor')
#plt.plot(encoder_time-(encoder_time[0]+(encoder_time[-1]-encoder_time[0])/len(encoder_time)*np.arange(len(encoder_time))), color='blue', label='encoder')
plt.plot(SAAC_h_time-(SAAC_h_time[0]+(SAAC_h_time[-1]-SAAC_h_time[0])/len(SAAC_h_time)*np.arange(len(SAAC_h_time))), color='black', label='SAAC_h')
plt.plot(SAAC_v_time-(SAAC_v_time[0]+(SAAC_v_time[-1]-SAAC_v_time[0])/len(SAAC_v_time)*np.arange(len(SAAC_v_time))), color='green', label='SAAC_v')
plt.legend()
plt.grid()
plt.show()



